#ifndef __SENSORS_FUSION_H__
#define __SENSORS_FUSION_H__

#include <mc_perception/sensor_base.h>
#include <mc_perception/undistortion/lidar_2d_undistortion.h>
#include <mc_perception/filter/point_cloud_filter.h>

// 二维雷达数据配置
struct Lidar2DConfig{
    SeneorConfig base_config;
    bool need_undistortion = true;      // 是否需要去畸变
    int max_points = 1080;              // 最大点集数据量
};


// 点云数据配置
struct PointCloudConfig{
    SeneorConfig base_config;
    bool need_undistortion = false;     // 是否需要去畸变
};


class SensorsFusion{
    public:
        SensorsFusion(int lidar2dNums, int pointcloud2Nums);

        // 添加雷达数据
        void addScan2DData(Lidar2DConfig config, Scan2DPtr scan_ptr);

        // 添加点云数据
        void addPointCloudData(PointCloudConfig config, PointCloud2Ptr pointcloud2_ptr);

        // 添加IMU数据
        void addIMUData(ImuPtr imu_ptr);

        // 添加里程计数据
        void addOdomData(OdometryPtr odom_ptr);

        // 获取融合数据
        PointCloud2Ptr getFusionData();

    private:
        Lidar2dUndistortion lidarUndistor_;
        int lidar2dNums_;         //二维雷达数量
        int pointcloud2Nums_;      //点云数量
        // 融合数据
        vector<PointCloudXYZPtr> pointcloudFusionDeque_;
};

#endif